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ABSTRACT 

Linear  and  nonlinear  constrained  estimation  is 
investigated  in  this  paper  as  an  optimal  method  to 
integrate  GPS  fixes  with  digital  maps  so  as  to  improve 
accuracy  and  reliability.  In  addition  to  emergency  location 
and  roadside  assistance,  the  integration  of  GPS  with 
digital  maps  becomes  an  increasingly  popular  application 
in  automotives  particularly  for  real-time  routing,  driving 
guidance,  and  street  prompting.  A  position  fix  is  obtained 
by  a  GPS  receiver,  which  may  be  subject  to  significant 
errors  in  urban  canyons  due  to  such  effects  as  multipath 
and  weak  signal,  whereas  a  digital  map  provides  the  road 
network  of  a  region  in  which  a  user  is  traveling.  When  the 
information  about  roads  is  as  accurate  as  (or  even  better 
than)  GPS  measurements,  it  is  desired  naturally  to 
incorporate  such  information  into  position  solution.  In  this 
paper,  roads  are  modeled  with  analytic  functions  and  its 
integration  (fusion)  with  a  GPS  position  fix  is  cast  as 
linear  and/or  nonlinear  state  constraints  in  an  optimization 
procedure.  Similarly,  the  velocity  estimates  and  the  road 
directions  are  treated  as  another  pair  of  constraints.  The 
constrained  optimization  is  then  solved  with  the 
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Lagrangian  multiplier,  leading  to  a  closed-form  solution 
for  linear  constraints  and  an  iterative  solution  for 
nonlinear  constraints.  Geometric  interpretations  of  the 
solutions  are  provided  for  simple  cases.  Computer 
simulation  results  are  presented  to  illustrate  the 
algorithms. 

INTRODUCTION 

With  the  rapid  build  up  of  the  geographic  information 
system  (GIS)  including  digital  road  maps  (DRM)  and 
digital  terrain  elevation  database  (DTED),  information 
about  roads  is  becoming  more  accurate,  up  to  date,  and 
more  accessible.  The  best  route  to  a  destination  can  be 
easily  found  on  the  internet.  Road  and  terrain  information 
has  been  used  in  the  past  for  navigation  via  terrain 
contour  matching.  An  increasingly  popular  use  of  digital 
maps  is  for  automobile  navigation  with  a  GPS  receiver 
[Bullock  et  al.,  2006]. 

This  paper  is  concerned  with  improving  the  kinematic 
state  (position  and  velocity)  of  a  user  vehicle  via 
integrating  (or  fusing)  its  GPS  fixes  with  the  information 
about  a  road  along  which  the  vehicle  is  believed  to  be 
traveling.  When  the  information  about  roads  is  as  accurate 
as  (or  even  better  than)  GPS  measurements,  it  is  desirable 
to  incorporate  such  information  (fusion)  into  an  integrated 
solution.  When  a  vehicle  travels  off-road  or  on  an 
unknown  road,  the  state  estimation  problem  is 
unconstrained.  However,  when  the  vehicle  is  traveling  on 
a  known  road,  be  it  straight  or  curved,  the  state  estimation 
problem  can  be  cast  as  constrained  with  the  road  network 
information  available  from  digital  road/terrain  maps. 
Without  making  full  use  of  this  additional  information 
about  the  state  constraints,  the  state  estimates,  even 
obtained  with  the  Kalman  filter,  cannot  be  truly  optimal. 

To  use  such  state  constraints,  previous  attempts  can  be  put 
into  several  groups.  The  first  group  is  to  incorporate  road 
information  into  the  state  estimation  process.  One 
technique  is  to  reduce  the  system  model  parameterization. 
Another  technique  is  to  translate  the  state  constraints  onto 
the  state  process  and/or  observation  noise  covariance 
matrix  for  the  estimation  filter  [Kirubarajan  et  al.,  2000]. 
Yet  another  technique  is  to  project  a  dynamic  system  onto 
linear  state  constraints  and  then  apply  the  Kalman  filter  to 
the  projected  systems  [Ko  and  Bitmead,  2006].  Similarly, 
for  nonlinear  state  constraints,  there  is  the  one¬ 
dimensional  (ID)  representation  of  a  target  motion  along 
a  curvilinear  road  [Yang,  Bakich,  Blasch,  2005].  The 
technique  to  model  bounded  random  variables  with 
truncated  densities  also  belongs  to  this  group,  which 
could  easily  work  with  nonlinear  filters  such  as  a  particle 
filter  [Ristic,  Arullampalam,  and  Gordon,  2004]. 

The  second  group  is  to  treat  state  constraints  as  perfect 
(pseudo)  measurements.  For  a  road  segment,  its  analytic 
model  not  only  constrains  the  target  position  but  also  the 


direction  of  the  velocity  vector.  Indeed,  the  velocity 
vector  is  closely  aligned  with  the  road  orientation  for  a 
linear  segment  and  with  the  tangent  vector  at  the  vehicle 
position  for  a  nonlinear  segment.  Furthermore,  an 
estimate  of  centripetal  acceleration  can  be  obtained  given 
the  curvature  and  the  vehicle  speed. 

In  the  third  group,  an  unconstrained  Kalman  filter 
solution  is  first  obtained  and  then  the  unconstrained  state 
estimate  is  projected  onto  the  constrained  surface.  This 
technique  can  also  be  viewed  as  post-processing 
(estimation  or  updating)  correction  [Simon,  and  Chia, 
2002],  track  to  road  fusion  [Yang  and  Blasch,  2006],  or 
GPS  and  map  integration  referred  to  as  in  this  paper.  In 
conventional  track  fusion,  two  or  more  tracks  are 
available,  each  consisting  of  an  estimate  of  the  underlying 
track  with  its  estimation  error  covariance.  The  fused  track 
is  typically  found  that  minimizes  the  sum  of  covariance- 
weighted  state  errors  squared  [Bar-Shalom  and  Fi,  1995; 
Blackman  and  Popoli,  1999].  In  contrast  to  this 
conventional  track  fusion  that  operates  on  individual  state 
values  (points),  fusion  with  road  involves  a  state  value  (a 
point)  and  a  subset  of  state  values  (an  interval).  In  this 
paper,  roads  are  modeled  with  analytic  functions  and  its 
fusion  with  a  GPS  fix  is  therefore  formulated  as  linear  or 
nonlinear  state  constraints  in  an  optimization  procedure. 

In  this  paper,  the  constrained  optimization  is  solved  with 
the  Fagrangian  multiplier,  leading  to  a  closed-form 
solution  for  linear  constraints  and  an  iterative  solution  for 
nonlinear  constraints.  In  the  latter  case,  we  present  a 
method  that  allows  for  the  use  of  second-order  nonlinear 
state  constraints.  The  method  can  provide  better 
approximation  to  higher  order  nonlinearities.  The  new 
method  is  based  on  a  computational  algorithm  that 
iteratively  finds  the  Fagrangian  multiplier.  The  use  of  a 
second-order  constraint  vs.  linearization  is  a  tradeoff 
between  reducing  approximation  errors  to  higher-order 
nonlinearities  and  keeping  the  problem  computationally 
tractable. 

The  GP S/map  integration  considered  in  this  map  is  a 
“point”  operation  in  the  sense  that  each  GPS  fix  is 
updated  onto  a  road.  It  is  possible  to  implement  an 
“interval”  operation  in  which  a  sequence  of  GPS  fixes 
(e.g.,  angular  turns  vs.  distance  traveled)  can  be  compared 
with  a  digital  road  map.  Another  way  to  use  map 
information  to  improve  navigation  in  a  stop-go  urban 
environment  is  to  perform  map-predictive  model  selection 
and  updating.  These  and  other  aspects  will  be  addressed 
in  a  future  paper. 

TRACK  FUSION  WITH  LINEAR  SEGMENTS 

When  a  road  segment  is  straight,  it  can  be  modeled  as  a 
linear  state  constraint.  In  this  section,  we  first  summarize 
the  results  for  linearly  constrained  state  estimation  [Simon 
and  Chia,  2002]  as  an  approach  to  GPS  integration  with 
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linear  road  segments.  We  then  show  that  this  linearly 
constrained  state  estimation  is  equivalent  to  use  of 
constraints  as  measurements  in  state  update.  Finally,  we 
provide  a  simple  geometric  interpretation  of  the  linearly 
constrained  state  estimation. 

Linearly  Constrained  State  Estimation 

Consider  a  linear  time-invariant  discrete-time  dynamic 
system  together  with  its  measurement  as 

xk+l=Axk+But+wk  (la) 

yk=Cxt+vt  (lb) 


implied  that  they  can  be  time-dependent,  leading  to 
piecewise  linear  constraints. 

The  constrained  Kalman  filter  according  to  [Simon  and 
Chia,  2002]  is  constructed  by  directly  projecting  the 
unconstrained  state  estimate  xk  onto  the  constrained 

surface  /=  {x:  Dx  =  d}.  It  is  formulated  as  the  solution  to 
the  problem 

x  =  arg min(x  -  x)TW(x-  x)  (4) 

x&S 

where  IF  is  a  symmetric  positive  definite  weighting 
matrix. 


where  the  underscore  indicates  a  vector  quantity,  the 
subscript  k  is  the  time  index,  x  is  the  state  vector,  u  is  a 
known  input,  y_  is  the  measurement,  and  w  and  y  are  state 
and  measurement  noise  processes,  respectively.  It  is 
implied  that  all  vectors  and  matrices  have  compatible 
dimensions,  which  are  omitted  for  simplicity. 

The  goal  is  to  find  an  estimate  denoted  by  xk  of  xk  given 

the  measurements  up  to  time  k  denoted  by  Yk  =  {y0?  • 
yj,}.  Under  the  assumptions  that  the  state  and 
measurement  noises  are  uncorrelated  zero-mean  white 
Gaussian  with  w  ~  N{0,  Qj  and  y  -  N {0,  7?}  where  Q  and  R 
are  positive  semi-definite  covariance  matrices,  the 
Kalman  filter  provides  an  optimal  estimator  in  the  form  of 
xk  =  E{xk  |  Yk}  [Anderson  and  Moore, 1979].  Starting 

from  an  initial  estimate  x0  =  £{x0}  and  its  estimation 
error  covariance  matrix  PQ  =  E{(x0  -  x0  )(x0  -  x0  )T } 

where  the  superscript  T  stands  for  matrix  transpose,  the 
Kalman  filter  equations  specify  the  propagation  of  xk  and 

Pk  over  time  and  the  update  of**  and  Pk  by  measurement 


Yk  as 

=Axk  +Buk 

(2a) 

Pk+x=APkAT+Q 

(2b) 

xk+i  =lk+l  +Kk+l(yM  -CxM) 

(2c) 

+ 

II 

* — H 

1 

+ 

n 

+ 

(2d) 

KM=PMCT(CPkCT  +R)-' 

(2e) 

where  xk+l  and  p  are  the  predicted  state  and  prediction 
error  covariance,  respectively. 

Now  in  addition  to  the  dynamic  system  of  (1),  we  are 
given  the  linear  state  constraint  equation 

Dxk  =  d_  (3) 

where  D  is  a  known  constant  matrix  of  full  rank  and  d  is  a 
known  vector.  The  number  of  rows  in  D  is  the  number  of 
constraints,  which  is  assumed  to  be  less  than  the  number 
of  states.  If  D  is  a  square  matrix,  the  state  is  fully 
constrained  and  thus  can  be  solved  by  inverting  (3). 
Although  no  time  index  is  given  to  D  and  d  in  (3),  it  is 


Based  on  the  Lagrangian  multiplier  technique,  the 
solution  to  the  constrained  optimization  in  (4)  is  given  by 

x  =  x-  WlDT  ( DWxDt yl  (Dx  -  d)  (5) 

Several  interesting  statistical  properties  of  the  constrained 
Kalman  filter  are  presented  in  [Simon  and  Chia,  2002]. 
This  includes  the  fact  that  the  constrained  state  estimate 
as  given  by  (5)  is  an  unbiased  state  estimate  for  the 
system  in  (1)  subject  to  the  constraint  in  (3)  for  a  known 
symmetric  positive  definite  weighting  matrix  W. 
Furthermore  when  W=  P~l ,  the  constrained  state  estimate 
has  a  smaller  error  covariance  than  that  of  the 
unconstrained  state  estimate,  and  it  is  actually  the  smallest 
for  all  constrained  Kalman  filters  of  this  type.  Similar 
results  hold  in  terms  of  the  trace  of  the  estimation  error 
covariance  matrix  when  W=  I. 


Linear  Road  Constraint  as  Pseudo  Measurement 

As  described  above,  the  linear  constrained  estimator  (5) 
can  be  obtained  by  different  methods.  It  is  shown  in  this 
section  that  it  is  also  equivalent  to  the  solution  where  the 
linear  state  constraints  are  considered  as  perfect  (pseudo) 
measurements. 


For  the  linear  time-invariant  discrete-time  dynamic 
system  (la)  and  its  measurement  (lb),  consider  the  linear 
state  constraint  (3)  as  another  measurement  to  the  system, 
which  can  be  used  to  perform  the  filter  measurement 
update  (2c)  and  (2d)  right  after  (lb)  without  the  filter  time 
propagation  (2a)  and  (2b)  (i.e.,  stay  the  same).  To  apply 
(2),  we  identify  the  following  equivalence: 

A  =  I,  B  =  0;  Q  =  0  (6a) 

C  =  D,R  =  0,yk  =  d  (6b) 


Given  the  unconstrained  solution  (xk,Pk),  the  prediction 
step  is  given  by  (2a)  and  (2b): 


(7a) 

(7b) 


The  Kalman  filter  gain  is  given  by: 
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K  k+\  =  PkDT  (DPkDTy]  (8) 

The  updated  state  and  error  covariance  becomes 

xM=xk+PkDT(DPkDTyl(d-Dxk)  (9a) 

PM  =  Pk-PkDT(DPkDT)'DPk  (9b) 

If  we  choose  w  =  Pkx ,  (9a)  becomes 

xk+l  =xk+W'D\DW-'DTy\d-Dxk)  (10a) 

=  xk-W-'DT(DW-'DTy'(Dxk-d)  (10b) 


which  is  exactly  the  same  as  the  solution  given  by  (5). 

Geometric  Interpretation 

It  is  proved  in  [Yang  and  Blasch,  2006]  that  the  linear 
constrained  estimation  is  the  orthogonal  projection  of  the 
unconstrained  estimate  onto  the  constrained  surface.  It 
provides  a  theoretical  justification  of  the  intuitive  practice 
of  finding  a  point  along  the  road  that  is  of  the  shortest 
distance.  The  result  thus  complements  [Simon  and  Chia, 
2002],  providing  an  interesting  geometrical  interpretation 
to  the  linear  constrained  estimation  by  estimate 
projection. 

TRACK  FUSION  WITH  NONLINEAR  SEGMENTS 

When  a  road  segment  is  curved,  it  can  be  modeled  as  a 
nonlinear  state  constraint.  In  this  section,  we  first  analyze 
the  linearizing  approach  and  the  associated  constraint 
approximation  error.  We  then  present  an  iterative  solution 
to  a  second  order  state  constraint.  Finally,  we  offer  a 
geometric  interpretation  of  a  solution  under  a  circular 
constraint  and  a  simple  approach  to  a  more  general 
second  order  state  constraint. 

Approximation  Errors  in  Constraint  Linearization 

To  deal  with  nonlinearity,  a  simple  approach  is  to  project 
the  unconstrained  state  estimate  onto  linearized  state 
constraints.  Once  the  constraints  are  linearized,  the  results 
presented  in  the  previous  section  for  linear  cases  can  be 
applied.  However,  linearization  introduces  constraint 
approximation  error,  which  is  a  function  of  the 
nonlinearity  and,  more  importantly,  of  the  point  around 
which  the  linearization  takes  place.  This  may  lead  to  an 
undesired  divergence  problem  as  analyzed  below. 

Consider  the  nonlinear  state  constraint  of  the  form 

g(x)=h  (11) 

We  can  expand  the  nonlinear  state  constraints  about  a 
constrained  state  estimate  X  and  for  the  ith  row  of  (11), 
we  have 

=  gi&^g'®T{x-x)  +  ^(x-x)T  g"(x)(x-x) 

H - h,  =  0  (12) 


where  the  superscripts  '  and  "  denote  the  first  and  second 
partial  derivatives. 

Keeping  only  the  first-order  terms,  some  rearrangement 
leads  to 

g'ixfx  ~  h-  g(xp-  g’(xf  x  (13) 

where  g(x)  =  [...g,(x)...]T,  d=  and  g'(x)  = 

[...g/(x)...].  An  approximate  linear  constraint  is  therefore 
formed  by  replacing  D  and  d  in  (3)  with  g'(x)T  and 
h  -  g(x)+  g'(x)Tx ,  respectively. 

Fig.  1  illustrates  this  linearization  process  and  identifies 
possible  errors  associated  with  linear  approximation  of  a 
nonlinear  state  constraint.  As  shown,  the  previous 
constrained  state  estimate  x  lies  somewhere  on  the 
constrained  surface  but  is  away  from  the  true  state  x.  The 
projection  of  the  unconstrained  state  estimate  x  onto  the 
approximate  linear  state  constraint  produces  the  current 
constrained  state  estimate  x+,  which  is  however  subject  to 
the  constraint  approximation  error.  Clearly,  the  further 
away  x”  is  from  x,  the  larger  error  is  the  approximation 
introduced.  More  critically,  such  an  approximately  linear 
constrained  estimate  may  not  satisfy  the  original 
nonlinear  constraint  specified  in  (25).  It  is  therefore 
desired  to  reduce  this  approximation-introduced  error  by 
including  higher-order  terms  while  keeping  the  problem 
computationally  tractable.  One  possible  approach  is 
presented  in  the  next  section. 

Iterative  Solution  to  Second-Order  Constraints 

Naturally  formed  roads  tend  to  have  more  bends  and  turns 
of  irregular  shapes  (high  nonlinearity).  Even  highways 
have  to  follow  terrain  contours  when  crossing  mountains. 
Locally,  however,  it  suffices  to  represent  a  curved  road 
segment  by  a  second-order  state  constraint  function  as 


=  xMx  +  mx  +  xm  +  m0  =  0  (14) 

which  can  be  viewed  as  a  second-order  approximation  to 
an  arbitrary  nonlinearity  in  a  digital  terrain  map. 

Similar  to  (4),  we  can  formulate  the  projection  of  an 
unconstrained  state  estimation  onto  a  nonlinear  constraint 
surface  as  the  constrained  least-square  optimization 
problem 

x  =  arg  min(z  -  H  x)T  (z-Hx)  (15a) 

X 

subject  to  g(x)  =  0  (15b) 

If  we  let  W  =  HtH  and  z  =  Hx ,  the  formulation  in  (15) 
becomes  the  same  as  in  (4).  In  a  sense,  (15)  is  a  more 
general  formulation  because  it  can  also  be  interpreted  as  a 
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nonlinear  constrained  measurement  update  or  a  projection 
in  the  predicted  measurement  domain. 

The  solution  to  the  constrained  optimization  (15)  can  be 
obtained  again  using  the  Lagrangian  multiplier  technique 
as 

x  =  GlV(I  +  XZTYyle(s i)  (16a) 


of  X  so  its  derivative  relative  to  X  vanishes,  e  =  0 .  The 
quadratic  constrained  solution  is  then  given  by 

X  =(W+AM)-‘Wx  (19a) 

where  the  Lagrangian  multiplier  X  is  obtained  iteratively 
as  in  (18)  with  the  corresponding  q(X)  and  q(X )  given  by 


q(A)  =  £ 


ef(A)af 

(1  +  ^,2)2 


where  G  is  an  upper  right  diagonal  matrix  resulting  from 
the  Cholesky  factorization  of  W  =  HTH  as 

W(=HtH)  =  GtG  (16c) 

V,  an  orthonormal  matrix,  and  27,  a  diagonal  matrix  with 
its  diagonal  elements  denoted  by  <jh  are  obtained  from  the 
singular  value  decomposition  (SVD)  of  the  matrix  LG'1  as 

LG'1  =  UZVT  (16d) 

where  U  is  the  other  orthonormal  matrix  of  the  SVD  and 
L  results  from  the  factorization  M  =  LJL,  and 

e(X)  =  [...  ex(X),  ...]  T  =  Vr(Gr)1(Hrz  -  Am)  (16e) 

t  =  [...ti...]T=VT(GT)-1m  (16f) 

As  a  nonlinear  equation  in  X,  it  is  difficult  to  find  a 
closed-form  solution  in  general  for  the  nonlinear  equation 
q(X)  =  0  in  (16b).  Numerical  root- finding  algorithms  may 
be  used  instead.  For  example,  the  Newton’s  method  is 
used  below.  Denote  the  derivative  of  q(X)  with  respect  to 
X  as 


qW  =  2£ 


where 


e,.  (A)e,.  (1  +  Aaj  )cr?  -  ef  (/l)cr4 
(1  i  /.<r;Y 

-i  ett,(  1  +  Act,2)  -  et(A )?,cr2 

f  (1  +  ^,2)2 


e  =[...  e.  ...]  T  =  -VT(GT)~Im 


Then  the  iterative  solution  for  X  is  given  by 


<19b) 

The  solution  of  (19)  is  also  called  the  constrained  least 
squares  [Moon  and  Stirling,  2000:  pp  765-766],  which 
was  previously  applied  for  the  joint  estimation  and 
calibration  [Yang  and  Lin,  2004].  Similar  techniques  have 
been  used  for  the  design  of  filters  for  radar  applications 
[Abromovich  and  Sverdlik,  1970]  and  in  robust  minimum 
variance  beamforming  [Lorenz  and  Boyd,  2006].  When  M 
=  0,  the  constraint  in  (14)  degenerates  to  a  linear  one.  The 
constrained  solution  is  still  valid.  However,  the  iterative 
solution  for  finding  X  is  no  longer  applicable  but  a  closed- 
form  solution  is  available  instead  as  given  in  (5). 

Geometric  Interpolation  for  Simple  Cases 

Consider  a  simple  example  where  a  target  travels  along  a 
circle.  For  this  case,  in  fact,  a  closed- form  solution  can  be 
derived.  Assume  that  W  =  I2,  M  =  I2,  m  =  0,  and  m0  =  -r2. 
The  nonlinear  constraint  can  be  equivalently  written  as 

xTx  =  r2  (20) 

The  quadratic  constrained  estimate  given  in  (19a) 
becomes 

x  =  (JV  +  AM)1  Wx  =  (l  +  A)~lx  (21) 

where  X  is  the  Lagrangian  multiplier. 

Bringing  (21)  back  to  (20)  gives 

3cr3c  =  (-L)r-L  =  r2  (22) 

-  -  i  +  r  i  +  a 


\+i  ~  \ 


starting  with  X0  =  0 


The  iteration  stops  when  \Xk+\-Xk\  <  r,  a  given  small  value 
or  the  number  of  iterations  reaches  a  pre-specified 
number.  Then  bringing  the  converged  Lagrangian 
multiplier  X  back  to  (16a)  provides  the  constrained 
optimal  solution. 


The  solution  for  X  is 


J*T  *  —  l~l2  1 


where  |||  stands  for  the  2-norm  or  length  for  the  vector. 
Bringing  the  solution  for  X  in  (23)  back  to  (21)  gives 


Now  consider  the  special  case  where  W  =  HTH ,  z  =  H% , 

and  m  =  0,  that  is,  a  quadratic  constraint  on  the  state. 
Under  these  conditions,  t  =  0  and  e  is  no  longer  a  function 


This  indicates  that  for  this  particular  case  with  a  circular 
constraint,  the  constraining  results  in  normalization. 
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This  further  suggests  a  simple  solution  for  some  practical 
applications.  When  a  vehicle  is  traveling  along  a  circular 
path  (or  approximately  so),  one  can  first  find  the 
equivalent  center  of  the  circle  around  which  to  establish  a 
new  coordinate  system,  then  express  the  unconstrained 
solution  in  the  new  coordinate  and  normalize  it  as  the 
constrained  solution.  This  solution  can  then  be  converted 
back  to  the  original  coordinates.  For  non-circular  but 
second-order  paths,  eigenvalue-based  scaling  may  be 
effected  following  coordinate  translation  and  rotation  in 
order  to  apply  this  circular  normalization.  Reverse 
operations  are  then  used  to  transform  back.  For 
applications  of  high  dimensionality,  the  scalar  iterative 
solution  of  (17)  may  be  more  efficient. 

SIMULATION  RESULTS 

The  simulation  presented  in  this  section  is  adapted  from 
[Yang  and  Blasch,  2006b].  More  results  can  be  found  in 
[Yang  and  Blasch,  2006a;  2006b].  In  this  example,  a 
ground  vehicle  is  assumed  to  travel  along  a  circular  road 
segment  as  shown  in  Fig.  1.  The  turn  center  is  chosen  as 
the  origin  of  the  x-y  coordinates  and  the  turn  radius  is  r  = 
100  m.  The  vehicle  maintains  a  constant  turn  rate  of  5.7 
deg/s  with  an  equivalent  linear  speed  of  10  m/s.  The 
initial  state  is 

&=0=[*  X  y  jf=[100m,  0m/s,  0ml0m/s]T  (25) 


latter  case,  a  cascaded  two  Kalman  filters  is  implied,  one 
for  the  GPS  receiver  and  the  other  for  the  integration 
filter. 

When  represented  in  a  Cartesian  coordinate  system,  a 
vehicle  traveling  along  a  curved  road  is  certainly  subject 
to  acceleration  in  both  the  x-  and  y-axis.  However,  no 
effort  was  made  in  this  simulation  to  optimize  the 
integration  filter  for  maneuver  but  merely  selecting  Q  and 
the  initial  conditions  so  as  to  focus  on  constraining  the 
estimates.  The  initial  state  is  selected  to  be  the  same  as  the 
true  state,  i.e.,  x0  =  x0  and  the  initial  estimation  error 

covariance  is  selected  to  be 

P0  =  diag([52  l2  52  l2])  (28) 

Fig.  2  shows  sample  trajectories  of  the  linear  constrained 
Kalman  filter.  There  are  5  curves  and  2  series  of  data 
points  in  the  figure.  The  true  state  is  represented  by  a 
series  of  dots  (•)  at  consecutive  sampling  instants,  which 
is  plotted  on  the  solid  line  being  the  road  segment.  The 
corresponding  measurements  are  a  series  of  circles  (o). 

The  estimates  of  the  unconstrained  Kalman  filter  are 
shown  as  the  connected  triangles  (A)  whereas  those  of 
linearly  constrained  Kalman  filters  are  shown  as  the 
connected  crosses  (x),  stars  (*),  and  pluses  (+)  for  three 
linear  approximations  of  the  nonlinear  constraint  of 
curved  road,  respectively. 


The  GPS  fixes  of  the  vehicle  are  available  at  a  sampling 
interval  of  T  =  1  s,  which  can  be  modeled  as  position 
measurements  of  the  vehicle  as 


Ik 


10  0  0 
0  0  10 


+  v. 


(26) 


In  the  first  approximation  (the  line  with  cross  x  labeled 
“linear  constraint  1”),  a  single  linearizing  point  at  6{  =  10° 
is  chosen  to  cover  the  entire  curved  road,  where  6  is  the 
angle  made  relative  to  the  x-axis,  positive  in  the  counter¬ 
clock  direction.  The  linearized  state  constraint  at  Qx  can  be 
written  as 


where  the  measurement  error  y  ~  11(0,  R)  is  a  zero-mean 
Gaussian  noise,  independent  in  the  x-  and  y-axis.  The 
covariance  matrix  R  =  diag([crrx 2  ay2])  uses  the  particular 
values  of  arx  =  ary  =  7  m  in  the  simulation. 

For  a  loosely  coupled  integration,  a  simple  discrete-time 
second-order  kinematic  model  is  used  by  the  integration 
Kalman  filter 


"1 

T 

0 

0" 

1  7^2 

2  1 

0  " 

0 

1 

0 

0 

T 

0 

3T+i  — 

0 

0 

1 

T 

Xk  + 

0 

1  7^2 

2  1 

0 

0 

0 

1 

0 

T 

where  the  process  noise  w  ~  N(0,  Q)  is  also  a  zero-mean 
Gaussian  noise,  independent  of  the  measurement  noise  y. 
The  covariance  matrix  Q  =  diag([c>2  <j ?])  uses  the 

particular  values  of  <j..  =  <j  =  0.32  m/s2  in  the 

x  y 

simulation.  The  GPS  fixes  in  (40)  are  assumed  to  be 
obtained  inside  a  GPS  receiver  either  by  a  nonlinear  least 
square  method  or  by  an  extended  Kalman  filter.  In  the 


cos  6X  0  sin  0X  0 
0  cos  6X  0  sin  6X 

Although  all  estimates  are  faithfully  projected  by  the 
constrained  filter  onto  this  linear  constraint,  tangential  to 
the  curve  at  the  linearizing  point,  it  runs  away  from  the 
true  trajectory  and  the  resulting  errors  continue  to  grow. 
The  apparent  divergence  is  caused  by  the  choice  of 
linearization. 

In  the  second  approximation  (the  line  with  star  *  labeled 
“linear  constraint  2”),  two  linearizing  points  at  6X=  15° 
and  =  80°  are  chosen  to  cover  the  curved  road  with  two 
linear  segments.  The  switching  point  from  one  linear 
segment  to  the  other  in  this  case  is  at  6=  45°.  As  shown, 
the  estimates  are  projected  onto  one  of  the  two  linear 
segments.  Except  near  the  corner  where  the  two  linear 
approximations  intersect  (which  is  far  away  from  both 
linearizing  points),  the  linear  constrained  estimates 
typically  outperform  the  unconstrained  estimates  (closer 
to  the  true  state).  This  is  better  illustrated  in  Fig.  3  where 
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nonlinear  constrained  tracking  solution 


>  X1 


True  State  X  Previous  Constrained  State  Estimate 

Unconstrained  State  Estimate  X  Current  Constrained  Sate  Estimate 


road  segment 
true  state 
measurements 
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linear  constraint  1 
linear  constraint  2 
linear  constraint  3 


Fig.  1.  Errors  in  Linear  Approximation  of  Nonlinear 
State  Constraints 


Fig.  2.  Sample  Trajectories  for  Linear  Constrained 
Kalman  Filter 


linear  constrained  position  absolute  errors 


nonlinear  constrained  tracking  solution 
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Fig.  3.  Linear  Constrained  Position  Errors  vs.  Time 


Fig.  4.  Sample  Trajectories  for  Nonlinear  Constrained 
Kalman  Filter 


nonlinear  constrained  position  absolute  errors 


iterative  solution  of  lagrangian  multiplier:  first  5  data  points 
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Fig.  5.  Nonlinear  Constrained  Position  Errors  vs.  Time  Fig.  6.  Convergence  in  Iterative  Lagrangian  Multiplier 
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the  upper  plot  is  for  the  absolute  position  error  in  x  while 
the  lower  plot  is  for  the  absolute  position  error  in  y,  both 
plotted  as  a  function  of  time. 

Still  with  two  linearizing  points  and  the  same  switching 
point  at  6  =  45°,  the  third  approximation  (the  line  with 
star  +  labeled  “linear  constraint  3”)  adjusts  linearizing 
points  to  =  20°  and  02  =  70°.  A  better  overall 
performance  is  achieved  as  shown  in  Fig.  3. 

It  is  clear  from  Fig.  2  that  a  nonlinear  constraint  can  be 
approximated  with  linear  constraints  in  a  piecewise 
fashion.  By  judicious  selection  of  the  number  of  linear 
segments  and  their  placement  (i.e.,  the  point  around 
which  to  linearize),  a  reasonably  good  performance  can 
be  expected.  In  the  limit,  a  nonlinear  function  is 
represented  by  a  piecewise  function  composed  of  an 
infinite  number  of  linear  segments.  This  naturally  leads  to 
the  use  of  nonlinear  constraints. 

Fig.  4  shows  sample  trajectories  of  the  nonlinear 
constrained  Kalman  filter.  There  are  2  curves  and  3  series 
of  data  points  in  the  figure.  The  true  state  is  still 
represented  by  a  series  of  dots  (•)  at  the  sampling  instants, 
which  is  plotted  on  the  solid  line  of  road  segment.  The 
corresponding  measurements  are  again  a  series  of  circles 
(o).  The  unconstrained  Kalman  filter  is  shown  as  the 
connected  crosses  (x)  whereas  the  estimates  of 
nonlinearly  constrained  Kalman  filters  are  shown  as  a 
series  of  connected  pluses  (+)  and  stars  (*)  for  two 
implementations,  respectively. 

The  first  implementation  (the  series  of  pluses  +)  only 
applies  the  nonlinear  constraint  to  the  position  estimate 
whereas  the  second  implementation  (the  series  of  stars  *) 
applies  constraints  to  both  the  position  and  velocity 
estimates.  In  fact,  we  encounter  a  hybrid  (mixed)  linear 
and  nonlinear  state  constraint  situation.  The  constrained 
position  estimate  is  given  by  (19)  for  the  quadratic  case 
(equivalent  to  (24)  for  a  circular  road).  Since  the  velocity 
direction  is  along  the  tangent  of  the  road  curve,  the 
constrained  velocity  estimate  is  obtained  by  the  following 
projection 

— constrained  unconstrained  ("^^0 

where  v  =  [x  yj  is  the  estimated  velocity  vector  and  ju  = 

[-sin  6  cosO]T  is  the  constrained  unit  direction  vector 
associated  with  the  constrained  position  at 
6  -  tan  _1(j)/x)  • 

For  simplicity,  the  unconstrained  estimation  error 
covariance  is  not  modified  in  the  present  simulation  after 
the  constrained  estimate  is  obtained  using  the  projection 
algorithms  in  (19)  and  (30).  The  implementation  is 
therefore  pessimistic  (suboptimal)  in  the  sense  that  it  does 
not  take  into  account  the  reduction  in  the  estimation  error 
covariance  brought  in  by  constraining.  One  consequence 


of  this  simplification  is  more  volatile  state  estimates.  To 
quantify  this  effect,  one  approach  is  to  project  the 
unconstrained  probability  density  function  (i.e.,  a  normal 
distribution  with  support  on  the  whole  state  space)  onto 
the  nonlinear  constraint.  Statistics  can  then  be  calculated 
from  the  constrained  probability  density  function  with  the 
constraint  as  its  support.  Again,  the  resulting  error  ellipse 
represented  by  the  covariance  matrix  is  only  an 
approximation  to  the  second  order. 

As  shown  in  Fig.  4,  both  the  nonlinear  constrained 
estimates  fall  onto  the  road  as  expected.  Overall  the 
position  and  velocity  constrained  estimates  are  better 
(closer  to  the  true  state)  than  the  position-only  constrained 
estimates.  This  is  illustrated  in  Fig.  5  where  the  upper  plot 
is  for  the  absolute  position  error  in  x  while  the  lower  plot 
is  for  the  absolute  position  error  in  y. 

A  Monte  Carlo  simulation  is  used  to  generate  the  RMS 
errors  of  state  estimation.  The  results  are  based  on  a  total 
of  100  runs  across  16  updates  and  summarized  in  Table  I. 
The  performance  improvement  of  the  nonlinear 
constrained  filter  over  the  linearized  constrained  filter  is 
demonstrated. 

Finally,  we  use  Fig.  6  to  show  an  example  of  the 
Lagrangian  multiplier  as  it  is  calculated  iteratively  using 
(19).  The  runs  for  five  unconstrained  state  estimates  are 
plotted  in  the  same  figure  and  to  make  it  fit,  the 
normalized  absolute  values  of  A  are  taken.  As  shown, 
starting  from  zero,  it  typically  takes  4  iterations  for  the 
algorithm  to  converge  in  the  example  presented. 


Table  I.  RMS  Estimation  Errors 


RMS  Estimation  Error 

Estimators 

Position  (m) 

Velocity  (m/s) 

Unconstrained 

8.3663 

4.2640 

Best  Linear  Constrained 

5.5386 

2.5466 

Nonlinear  Constrained 

1.8056 

0.4252 

CONCLUSIONS 

In  this  paper,  we  presented  an  approach  to  incorporating 
road  information  into  navigation  solution  via  GP S/map 
integration.  In  this  approach,  road  segments  were 
modeled  with  analytic  functions  and  their  fusion  with  a 
position  fix  was  cast  as  a  linearly  or  nonlinearly  state 
constrained  optimization  procedure.  With  the  Lagrangian 
multiplier,  a  closed-form  solution  was  found  for  linear 
constraints  and  an  iterative  solution  for  nonlinear 
constraints.  Geometric  interpretations  of  the  solutions 
were  provided  for  simple  cases.  Computer  simulation 
results  demonstrate  the  performance  of  the  algorithms. 

Future  work  includes  both  algorithms  development  and 
practical  applications.  It  is  of  interest  to  extend  the 
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iterative  method  presented  in  the  paper  for  second-order 
nonlinear  state  constraints  to  other  types  of  nonlinear 
constraints  of  practical  significance  and  to  search  for 
more  efficient  root-finding  algorithms  to  solve  for  the 
Lagrangian  multiplier.  Similarly,  the  simple  fusion  to  a 
single  road  as  presented  in  this  paper  is  being  extended  to 
an  urban  environment  with  a  vehicle  moving  along 
closely-spaced  road  networks  with  intersections  and  by¬ 
passes.  In  this  case,  the  fusion  (or  constraining)  can  take 
place  in  the  measurement  level  as  well  as  in  the  position 
level,  involving  road  constrained  data  association 
(RCDA)  and  map-predictive  model  selection  and 
updating.  Results  will  be  reported  in  future  papers. 
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